#include "Registration.h"
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/transforms.h>
// #include "pcl/features/normal_3d.h"
// #include "pcl/features/fpfh.h"
// #include "pcl/registration/ppf_registration.h"
// #include "pcl/registration/ia_ransac.h"
#include <windows.h>


#define PATT_BLOCK_SIZE 3.15f


Registration::Registration ()
{
}

Registration::~Registration ()
{
}

void Registration::registration (std::vector< Vertex > & src,
		std::vector< Vertex > & target, std::vector< Vertex > & dst,
		std::vector< int > &srcNearTargetIndices,
		std::vector< int > &targetNearSrcIndices,
		std::vector< Vertex > & srcNearTargetPoints,
		std::vector< Vertex > & targetNearSrcPoints)
{
	PointXYZCloud::Ptr srcCloud (new PointXYZCloud);
	PointXYZCloud::Ptr targetCloud (new PointXYZCloud);
	PointXYZCloud::Ptr srcCuttedCloud (new PointXYZCloud);
	PointXYZCloud::Ptr targetCuttedCloud (new PointXYZCloud);
	PointXYZCloud::Ptr srcNearTargetCloud (new PointXYZCloud);
	PointXYZCloud::Ptr targetNearSrcCloud (new PointXYZCloud);
	PointXYZCloud::Ptr DstCloud (new PointXYZCloud);

	OutputDebugString (L"Enter Registration\n");
	
	_glmToPcl (src, srcCloud);
	_glmToPcl (target, targetCloud);
	OutputDebugString (L"Convert to Cloud complete\n");

	_cutCloud (srcCloud, srcCuttedCloud);
	_cutCloud (targetCloud, targetCuttedCloud);
	OutputDebugString (L"Cut Cloud complete\n");

	_downSampling (srcCuttedCloud, srcCuttedCloud, 1.0f);
 	_downSampling (targetCuttedCloud, targetCuttedCloud, 1.0f);
	OutputDebugString (L"Down sampling complete\n");

	_getNearTargetPoints (srcCuttedCloud, targetCuttedCloud, srcNearTargetCloud, srcNearTargetIndices);
//	_getNearTargetPoints (targetCloud, srcCloud, targetNearSrcCloud, targetNearSrcIndices);
// 	_getNearTargetPoints (srcCuttedCloud, targetCuttedCloud, srcNearTargetCloud, srcNearTargetIndices);
// 	_getNearTargetPoints (targetCuttedCloud, srcCuttedCloud, targetNearSrcCloud, targetNearSrcIndices);
	OutputDebugString (L"Get near target points complete\n");

// 	pcl::transformPointCloud (*srcCloud, *DstCloud,
// 			_doICP (srcNearTargetCloud, targetCloud));
// 	FeatureCloud srcFeatureCloud, targetFeatureCloud;
// 	srcFeatureCloud.setInputCloud (srcNearTargetCloud);
// 	targetFeatureCloud.setInputCloud (targetCuttedCloud);
// 	pcl::transformPointCloud (*srcCloud, *DstCloud,
// 			_doSAC (&srcFeatureCloud, &targetFeatureCloud));

	pcl::transformPointCloud (*srcCloud, *DstCloud,
			_doAlign (srcNearTargetCloud, targetCuttedCloud));
	OutputDebugString (L"Transform complete\n");

 	_pclToGlm (DstCloud, src);
	_pclToGlm (srcNearTargetCloud, srcNearTargetPoints);
// 	_pclToGlm (targetNearSrcCloud, targetNearSrcPoints);
// 	OutputDebugString (L"Convert to glm vertices complete\n");
}

#include <pcl/registration/ndt.h>
Eigen::Matrix4f Registration::_doAlign (PointXYZCloud::Ptr srcCloud,
		PointXYZCloud::Ptr targetCloud)
{
	pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr dstCloud (new pcl::PointCloud< pcl::PointXYZRGBNormal >);
	pcl::NormalDistributionsTransform< pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal > ndt;
	clock_t startTime = clock ();
	ndt.setMaximumIterations (50);
	ndt.setMaxCorrespondenceDistance (1.5f * PATT_BLOCK_SIZE);
	ndt.setTransformationEpsilon (1000.0);
	ndt.setOptimizationStepSize (0.01);
	ndt.setGridStep (Eigen::Vector2f (1.0, 1.0));

	ndt.setInputCloud (srcCloud);
	ndt.setInputTarget (targetCloud);
	ndt.align (*dstCloud, Eigen::Matrix4f::Identity ());
	clock_t endTime = clock ();
	Eigen::Matrix4f fm = ndt.getFinalTransformation ();
	double fitness = ndt.getFitnessScore ();

	std::wstringstream sout;
	sout << "NDT complete: " << (endTime - startTime) << "ms" << std::endl;
	sout << "has converged:" << ndt.hasConverged () << " score: " <<
		ndt.getFitnessScore () << std::endl;
	OutputDebugString (sout.str ().c_str ());

	return fm;
}

// #include "pcl/registration/icp.h"
// Eigen::Matrix4f Registration::_doAlign (PointXYZCloud::Ptr srcCloud,
// 		PointXYZCloud::Ptr targetCloud)
// {
// 	pcl::PointCloud< pcl::PointXYZRGBNormal > Final;
// 	pcl::IterativeClosestPoint< pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal > icp;
// 	clock_t startTime = clock ();
// 	icp.setMaximumIterations (50);
// 	icp.setEuclideanFitnessEpsilon (0.00001);
// 	icp.setMaxCorrespondenceDistance (1.5f * PATT_BLOCK_SIZE);
// 
// 	icp.setInputCloud (srcCloud);
// 	icp.setInputTarget (targetCloud);
// 	icp.align (Final, Eigen::Matrix4f::Identity ());
// 	clock_t endTime = clock ();
// 
// 	std::wstringstream sout;
// 	sout << "ICP complete: " << (endTime - startTime) << "ms" << std::endl;
// 	sout << "has converged:" << icp.hasConverged() << " score: " <<
// 			icp.getFitnessScore () << std::endl;
// 	OutputDebugString (sout.str ().c_str ());
// 
// 	return icp.getFinalTransformation ();
// }

void Registration::_glmToPcl (std::vector< Vertex > &input,
		PointXYZCloud::Ptr output)
{
	output->width = input.size ();
	output->height = 1;
	output->is_dense = false;
	output->points.resize (output->width * output->height);
	for (size_t i = 0; i < output->points.size (); ++i) {
		output->points[i].x = input[i].point.x;
		output->points[i].y = input[i].point.y;
		output->points[i].z = input[i].point.z;
		output->points[i].normal_x = input[i].normal.x;
		output->points[i].normal_y = input[i].normal.y;
		output->points[i].normal_z = input[i].normal.z;
		output->points[i].r = input[i].color.x * 255.0f;
		output->points[i].g = input[i].color.y * 255.0f;
		output->points[i].b = input[i].color.z * 255.0f;
	}
}

void Registration::_pclToGlm (PointXYZCloud::Ptr input,
		std::vector< Vertex > &output)
{
	output.resize (input->points.size ());
	for (size_t i = 0; i < input->points.size (); ++i) {
		output[i].point.x = input->points[i].x;
		output[i].point.y = input->points[i].y;
		output[i].point.z = input->points[i].z;
		output[i].normal.x = input->points[i].normal_x;
		output[i].normal.y = input->points[i].normal_y;
		output[i].normal.z = input->points[i].normal_z;
		output[i].color.r = input->points[i].r / 255.0f;
		output[i].color.g = input->points[i].g / 255.0f;
		output[i].color.b = input->points[i].b / 255.0f;
	}
}

void Registration::_getNearTargetPoints (PointXYZCloud::Ptr srcCloud,
		PointXYZCloud::Ptr targetCloud, PointXYZCloud::Ptr srcNearTargetCloud,
		std::vector< int > &resultIndices)
{
	pcl::search::KdTree< pcl::PointXYZRGBNormal > searchMethod;
	searchMethod.setInputCloud (targetCloud);
	resultIndices.clear ();
	for (size_t i = 0; i < srcCloud->size (); ++i) {
		std::vector< int > resultNeighIndices;
		std::vector< float > resultNeighDistances;
		searchMethod.radiusSearch (srcCloud->at (i), 1.5f * PATT_BLOCK_SIZE,
				resultNeighIndices, resultNeighDistances);
		if (resultNeighIndices.empty () == false) {
			srcNearTargetCloud->push_back ((*srcCloud)[i]);
			resultIndices.push_back (i);
		}
	}
}

void Registration::_cutCloud (PointXYZCloud::Ptr src, PointXYZCloud::Ptr result)
{
	for (size_t i = 0; i < src->size (); ++i) {
		if ((*src)[i].x >= -100.0 && (*src)[i].x <= 100.0 &&
				(*src)[i].y >= -100.0 && (*src)[i].y <= 100.0 &&
				(*src)[i].z >= -100.0 && (*src)[i].z <= 100.0) {
// 			if ((*src)[i].x >= -2.0 * PATT_BLOCK_SIZE && (*src)[i].x <= 9.0 * PATT_BLOCK_SIZE &&
// 					(*src)[i].y >= -2.0 * PATT_BLOCK_SIZE && (*src)[i].y <= 7.0 * PATT_BLOCK_SIZE &&
// 					(*src)[i].z >= -0.5 * PATT_BLOCK_SIZE && (*src)[i].z <= 0.5 * PATT_BLOCK_SIZE)
// 				continue;
			result->push_back ((*src)[i]);
		}
	}
}

void Registration::_downSampling (PointXYZCloud::Ptr srcCloud,
		PointXYZCloud::Ptr resultCloud, float step)
{
	pcl::ApproximateVoxelGrid< pcl::PointXYZRGBNormal > approximate_voxel_filter;
	approximate_voxel_filter.setLeafSize (step, step, step);
	approximate_voxel_filter.setInputCloud (srcCloud);
	approximate_voxel_filter.filter (*resultCloud);
}

// Eigen::Matrix4f Registration::_doSAC (FeatureCloud *src, FeatureCloud *target)
// {
// 	pcl::SampleConsensusInitialAlignment< pcl::PointXYZRGBNormal,
// 			pcl::PointXYZRGBNormal, pcl::FPFHSignature33> sac_ia_;
// 	float min_sample_distance_ = 0.05f;
// 	float max_correspondence_distance_ = 0.01f * 0.01f;
// 	int nr_iterations_ = 30;
// 	sac_ia_.setMinSampleDistance (min_sample_distance_);
// 	sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
// 	sac_ia_.setMaximumIterations (nr_iterations_);
// 
// 	sac_ia_.setInputCloud (src->getPointCloud ());
// 	sac_ia_.setSourceFeatures (src->getLocalFeatures ());
// 	sac_ia_.setInputTarget (target->getPointCloud ());
// 	sac_ia_.setTargetFeatures (target->getLocalFeatures ());
// 
// 	pcl::PointCloud< pcl::PointXYZRGBNormal > registration_output;
// 	sac_ia_.align (registration_output);
// 
// 	float fitness_score = (float) sac_ia_.getFitnessScore (
// 			max_correspondence_distance_);
// 	return sac_ia_.getFinalTransformation ();
// 
// }
// 

// Eigen::Matrix4f Registration::_doPPF (PointXYZCloud::Ptr srcCloud,
// 		PointXYZCloud::Ptr targetCloud)
// {
// 	pcl::PPFRegistration<pcl::PointXYZ, pcl::PointXYZ> ppf;
// 	ppf.setInputCloud (srcCloud);
// 	ppf.setInputTarget (targetCloud);
// 	ppf.setMaximumIterations (20);
// 
// 	pcl::PointCloud<pcl::PointXYZ> Final;
// 	ppf.align(Final);
// 
// 	std::wstring fitnessStr;
// 	std::wstringstream sout;
// 	sout << "has converged:" << ppf.hasConverged() << " score: " <<
// 		ppf.getFitnessScore() << std::endl;
// 	OutputDebugString (sout.str ().c_str ());
// 	return ppf.getFinalTransformation();
// }

// 	// Initializing point cloud visualizer
// 	boost::shared_ptr<pcl::visualization::PCLVisualizer>
// 		viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
// 	viewer_final->setBackgroundColor (0, 0, 0);
// 
// 	// Coloring and visualizing target cloud (red).
// 	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
// 		target_color (target_cloud, 255, 0, 0);
// 	viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
// 	viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
// 		1, "target cloud");
// 
// 	// Coloring and visualizing transformed input cloud (green).
// 	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
// 		output_color (output_cloud, 0, 255, 0);
// 	viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
// 	viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
// 		1, "output cloud");
// 
// 	// Starting visualizer
// 	viewer_final->addCoordinateSystem (1.0);
// 	viewer_final->initCameraParameters ();
// 
// 	// Wait until visualizer window is closed.
// 	while (!viewer_final->wasStopped ())
// 	{
// 		viewer_final->spinOnce (100);
// 		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
// 	}
// class FeatureCloud
// {
// public:
// 	// A bit of shorthand
// 	typedef pcl::PointCloud< pcl::PointXYZRGBNormal > PointCloud;
// 	typedef pcl::PointCloud< pcl::Normal > SurfaceNormals;
// 	typedef pcl::PointCloud< pcl::FPFHSignature33 > LocalFeatures;
// 	typedef pcl::search::KdTree< pcl::PointXYZRGBNormal > SearchMethod;
// 
// 	FeatureCloud () :
// 	search_method_xyz_ (new SearchMethod),
// 		normal_radius_ (0.02f),
// 		feature_radius_ (0.02f)
// 	{}
// 
// 	~FeatureCloud () {}
// 
// 	// Process the given cloud
// 	void
// 		setInputCloud (PointCloud::Ptr xyz)
// 	{
// 
// 		xyz_ = xyz;
// 		processInput ();
// 	}
// 
// 	// Get a pointer to the cloud 3D points
// 	PointCloud::Ptr
// 		getPointCloud () const
// 	{
// 		return (xyz_);
// 	}
// 
// 	// Get a pointer to the cloud of 3D surface normals
// 	SurfaceNormals::Ptr
// 		getSurfaceNormals () const
// 	{
// 		return (normals_);
// 	}
// 
// 	// Get a pointer to the cloud of feature descriptors
// 	LocalFeatures::Ptr
// 		getLocalFeatures () const
// 	{
// 		return (features_);
// 	}
// 
// protected:
// 	// Compute the surface normals and local features
// 	void
// 		processInput ()
// 	{
// 		computeSurfaceNormals ();
// 		computeLocalFeatures ();
// 	}
// 
// 	// Compute the surface normals
// 	void
// 		computeSurfaceNormals ()
// 	{
// 		normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
// 
// 		pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::Normal> norm_est;
// 		norm_est.setInputCloud (xyz_);
// 		norm_est.setSearchMethod (search_method_xyz_);
// 		norm_est.setRadiusSearch (normal_radius_);
// 		norm_est.compute (*normals_);
// 	}
// 
// 	// Compute the local feature descriptors
// 	void
// 		computeLocalFeatures ()
// 	{
// 		features_ = LocalFeatures::Ptr (new LocalFeatures);
// 
// 		pcl::FPFHEstimation<pcl::PointXYZRGBNormal, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
// 		fpfh_est.setInputCloud (xyz_);
// 		fpfh_est.setInputNormals (normals_);
// 		fpfh_est.setSearchMethod (search_method_xyz_);
// 		fpfh_est.setRadiusSearch (feature_radius_);
// 		fpfh_est.compute (*features_);
// 	}
// 
// private:
// 	// Point cloud data
// 	PointCloud::Ptr xyz_;
// 	SurfaceNormals::Ptr normals_;
// 	LocalFeatures::Ptr features_;
// 	SearchMethod::Ptr search_method_xyz_;
// 
// 	// Parameters
// 	float normal_radius_;
// 	float feature_radius_;
// };
